//
//  limb.cpp
//  template4
//
//  Created by Xiaochen Lian on 5/15/12.
//  Copyright (c) 2012 University of California, Los Angeles. All rights reserved.
//

#include "limb.h"

//----------------------------------------------------------------------------
// ILimb
ILimb::ILimb(const vec4 &pos, GLfloat length)
{
    m_length = length;
    set_pos(pos + vec4(length/2, 0.0, 0.0, 0.0));
    m_tfm = mat4(1.0);
}

void ILimb::set_material(const lighting_struct &material)
{
    m_material = material;
}

void ILimb::draw(const mat4 &tfm_parent) 
{
    glUniform4fv(uAmbient, 1, g_env_lighting.ambient * m_material.ambient);
    glUniform4fv(uDiffuse, 1, g_env_lighting.diffuse.x * m_material.diffuse);
    glUniform4fv(uSpecular, 1, g_env_lighting.specular * m_material.specular);
    glUniform1f(uShininess, g_env_lighting.shininess * m_material.shininess);

    mat4 m = tfm_parent * m_model_mat * m_tfm;
    m_mass_center = m * vec4(0.0, 0.0, 0.0, 1.0);
    drawCube(m * m_scale_mat);
    
    for (int i = 0; i < m_children.size(); ++i)
        m_children[i]->draw(m);
}

inline bool ILimb::is_right()
{
    return m_length < 0;
}

//----------------------------------------------------------------------------
// CArm
CArm::CArm(const vec4 &pos, GLfloat length)
:ILimb(pos, length)
{
    m_type = SHOULDER;
}

void CArm::build_model(const point4 &parent_mass_center)
{
    m_model_mat = Translate(m_model_pos);
    m_scale_mat = Scale(abs(m_length), 1.0f, 1.0f);
    
    m_material.ambient = color4( 0.4f, 0.5f, 0.4f, 1.0f );
    m_material.diffuse = color4( 0.4f, 0.5f, 0.4f, 1.0f );
    m_material.specular = color4( 0.1f, 0.1f, 0.1f, 1.0f );
    m_material.shininess = 10.0f;

    m_mass_center = m_model_mat * parent_mass_center;
}

mat4 CArm::tfm_from_status(const bird_status &nstatus)
{
    mat4 next_tfm;
    
    mat4 T1 = Translate(m_length/2, 0.0, 0.0);
    mat4 T2 = Translate(-m_length/2, 0.0, 0.0);
    
    if (is_right())
    {
        next_tfm = RotateX(nstatus.rshoulder_twist[0]);
        next_tfm = RotateZ(nstatus.rshoulder_dihedral[0]) * T1 * next_tfm;
        next_tfm = T2 * RotateY(-nstatus.shoulder_sweep[0]) * next_tfm;
    }
    else {
        next_tfm = RotateX(nstatus.lshoulder_twist[0]);
        next_tfm = RotateZ(nstatus.lshoulder_dihedral[0]) * T1 * next_tfm;
        next_tfm = T2 * RotateY(nstatus.shoulder_sweep[0]) * next_tfm;
    }
    
    return next_tfm;
}

vec4 CArm::aerodynamics(const bird_status &nstatus, const mat4 &tfm, const double timestep)
{
    vec4 force = 0.0;
    
    m_next_tfm = tfm_from_status(nstatus);
    for (int i = 0; i < m_children.size(); ++i)
        force += m_children[i]->aerodynamics(nstatus, tfm * m_model_mat * m_next_tfm, timestep);
    
    return force;
}

void CArm::update(const bird_status &cstatus, const mat4 tfm_parent) 
{
    m_tfm = m_next_tfm;
    mat4 m = tfm_parent * m_model_mat * m_tfm;
    m_mass_center = m * vec4(0.0, 0.0, 0.0, 1.0);;
        
    for (int i = 0; i < m_children.size(); ++i)
        m_children[i]->update(cstatus, m);
}

void CArm::init(const bird_status &cstatus)
{
    m_next_tfm = tfm_from_status(cstatus);
    for (int i = 0; i < m_children.size(); ++i)
        m_children[i]->init(cstatus);
}


////----------------------------------------------------------------------------
//// CForearm
//CForearm::CForearm(const vec4 &pos, GLfloat length)
//:ILimb(pos, length)
//{
//    m_type = FOREARM;
//}
//
//void CForearm::build_model()
//{
//    m_tfm = Translate(m_model_pos);
//    m_scale_mat = Scale(abs(m_length), 1.0f, 1.0f);
//    
//    m_material.ambient = color4( 0.5f, 0.4f, 0.4f, 1.0f );
//    m_material.diffuse = color4( 0.5f, 0.4f, 0.4f, 1.0f );
//    m_material.specular = color4( 0.1f, 0.1f, 0.1f, 1.0f );
//    m_material.shininess = 10.0f;
//}
//
//void CForearm::update(const bird_status &status) 
//{
//    mat4 T1 = Translate(m_length/2, 0.0, 0.0);
//    mat4 T2 = Translate(-m_length/2, 0.0, 0.0);
//    
//    if (is_right())
//        m_tfm = T2 * RotateY(status.relbow_bend[0]) * T1;
//    else {
//        m_tfm = T2 * RotateY(status.lelbow_bend[0]) * T1;
//    }
//    
//    for (int i = 0; i < m_children.size(); ++i)
//        m_children[i]->update(status);
//}
//

//----------------------------------------------------------------------------
// CWrist
CWrist::CWrist(const vec4 &pos, GLfloat length)
:ILimb(pos, length)
{
    m_type = WRIST;
}

void CWrist::build_model(const point4 &parent_mass_center)
{
    m_model_mat = Translate(m_model_pos);
    m_scale_mat = Scale(abs(m_length), 1.0f, 1.0f);
    
    m_material.ambient = color4( 0.4f, 0.4f, 0.5f, 1.0f );
    m_material.diffuse = color4( 0.4f, 0.4f, 0.5f, 1.0f );
    m_material.specular = color4( 0.1f, 0.1f, 0.1f, 1.0f );
    m_material.shininess = 10.0f;

    m_mass_center = m_model_mat * parent_mass_center;
}

mat4 CWrist::tfm_from_status(const bird_status &nstatus)
{
    mat4 next_tfm; 
    mat4 T1 = Translate(m_length/2, 0.0, 0.0);
    mat4 T2 = Translate(-m_length/2, 0.0, 0.0);
    
    if (is_right()) 
    {
        next_tfm = RotateX(nstatus.rwrist_twist[0]);
        next_tfm = T2 * RotateY(nstatus.rwrist_bend[0]) * T1 * next_tfm;
    }
    else {
        next_tfm = RotateX(nstatus.lwrist_twist[0]);
        next_tfm = T2 * RotateY(nstatus.lwrist_bend[0]) * T1 * next_tfm;
    }

    return next_tfm;
}

vec4 CWrist::aerodynamics(const bird_status &nstatus, const mat4 &tfm, const double timestep)
{
    vec4 force = 0.0;
        
    m_next_tfm = tfm_from_status(nstatus);
    for (int i = 0; i < m_children.size(); ++i)
        force += m_children[i]->aerodynamics(nstatus, tfm * m_model_mat * m_next_tfm, timestep);
    
    return force;
}


void CWrist::update(const bird_status &cstatus, const mat4 tfm_parent) 
{   
    m_tfm = m_next_tfm;
    mat4 m = tfm_parent * m_model_mat * m_tfm;
    m_mass_center = m * vec4(0.0, 0.0, 0.0, 1.0);
    
    for (int i = 0; i < m_children.size(); ++i)
        m_children[i]->update(cstatus, m);
}

void CWrist::init(const bird_status &cstatus)
{
    m_next_tfm = tfm_from_status(cstatus);
    for (int i = 0; i < m_children.size(); ++i)
        m_children[i]->init(cstatus);
}
